#include "lidar_ai_data.h"
#include <fstream>
#include <sstream>
#include <regex>


std::vector<LiDARFrame> loadLiDARData(const std::string& path) {
    std::ifstream file(path);
    std::vector<LiDARFrame> lidar_frames;
    std::string line;

    if (!file.is_open()) {
        std::cerr << "Failed to open LiDAR file: " << path << std::endl;
        return lidar_frames;
    }

    LiDARFrame frame;

    while (std::getline(file, line)) {
        if (line.empty()) continue;

        if (line.find("time_stamp:") == 0) {
            if (frame.time_stamp != 0) {
                lidar_frames.push_back(frame);
                frame = LiDARFrame(); // 重置
            }
            frame.time_stamp = std::stoll(line.substr(11));
        }
        else if (line.find("fusepose:") == 0) {
            std::istringstream iss(line.substr(9));
            float x, y, z;
            iss >> x >> y >> z;
            frame.fusepose = cv::Point3f(x, y, z);
        }
        else if (line.find("rpy:") == 0) {
            std::istringstream iss(line.substr(4));
            float roll, pitch, yaw;
            iss >> roll >> pitch >> yaw;
            frame.rpy = cv::Point3f(roll, pitch, yaw);
        }
    }

    if (frame.time_stamp != 0) {
        lidar_frames.push_back(frame);
    }

    return lidar_frames;
}

std::vector<AIFrame> loadAIData(const std::string& path) {
    std::ifstream file(path);
    std::vector<AIFrame> ai_frames;
    std::string line;

    if (!file.is_open()) {
        std::cerr << "Failed to open AI file: " << path << std::endl;
        return ai_frames;
    }

    AIFrame frame;

    while (std::getline(file, line)) {
        if (line.empty()) continue;

        if (line.find("time_stamp:") == 0) {
            if (frame.time_stamp != 0) {
                ai_frames.push_back(frame);
                frame = AIFrame();
            }
            frame.time_stamp = std::stoll(line.substr(11));
        }
        else if (line.find("xyz:") == 0) {
            std::istringstream iss(line.substr(4));
            float x, y, z;
            iss >> x >> y >> z;
            frame.xyz.emplace_back(x, y, z);
        }
    }

    if (frame.time_stamp != 0) {
        ai_frames.push_back(frame);
    }

    return ai_frames;
}

std::vector<MatchedFrame> matchingPoseWithAI(const std::vector<AIFrame>& ai_data, const std::vector<LiDARFrame>& li_data) {
    std::vector<MatchedFrame> result;
    size_t i = 0, j = 0;
    const long long threshold = 50; // 时间戳匹配阈值
    size_t index = 0;

    while (i < li_data.size() && j < ai_data.size()) {
        long long diff = li_data[i].time_stamp - ai_data[j].time_stamp;

        if (std::abs(diff) < threshold) {
            if (!ai_data[j].xyz.empty()) {
                // std::cout << index << " " << ai_data[j].time_stamp << std::endl;
            }

            MatchedFrame tmp;
            tmp.lidar_time_stamp = li_data[i].time_stamp;
            tmp.ai_time_stamp = ai_data[j].time_stamp;
            tmp.fusepose = li_data[i].fusepose;
            tmp.rpy = li_data[i].rpy;
            tmp.xyz = ai_data[j].xyz;
            tmp.diff = diff;
            tmp.pcl_no = ai_data[j].xyz.size();

            result.push_back(tmp);
            index++;
            j++; // 移动 AI 数据指针
        } else if (diff < 0) {
            i++; // LiDAR 数据时间戳较小，移动 LiDAR 数据指针
        } else {
            j++; // AI 数据时间戳较小，移动 AI 数据指针
        }
    }

    return result;
}


void findMinMaxXY(const std::vector<MatchedFrame>& frames, 
                  float& min_x, float& max_x, 
                  float& min_y, float& max_y) {
    min_x = min_y = std::numeric_limits<float>::max();
    max_x = max_y = std::numeric_limits<float>::lowest();

    for (const auto& frame : frames) {
        auto pt = frame.fusepose;
        {
            if (pt.x < min_x) min_x = pt.x;
            if (pt.x > max_x) max_x = pt.x;
            if (pt.y < min_y) min_y = pt.y;
            if (pt.y > max_y) max_y = pt.y;
        }
    }
}


std::vector<MatchedFrame> readAndParseData(const std::string& file_name) {
    std::ifstream file(file_name);
    std::string line;
    std::vector<MatchedFrame> formatted_frames;

    if (!file.is_open()) {
        std::cerr << "無法打開文件: " << file_name << std::endl;
        return formatted_frames; // 返回空的向量
    }

    long long lidar_time_stamp = 0;
    long long ai_time_stamp = 0;
    cv::Point3f fusepose;
    cv::Point3f rpy;

    while (std::getline(file, line)) {
        std::smatch match;

        if (line.find("fusepose:") != std::string::npos) {
            std::regex fusepose_regex(R"(([-+]?\d*\.\d+|[-+]?\d+))");
            std::vector<float> fusepose_data;
            auto words_begin = std::sregex_iterator(line.begin(), line.end(), fusepose_regex);
            auto words_end = std::sregex_iterator();

            for (std::sregex_iterator i = words_begin; i != words_end; ++i) {
                fusepose_data.push_back(std::stof((*i).str()));
            }

            lidar_time_stamp = static_cast<long long>(fusepose_data[0]);
            fusepose = cv::Point3f(fusepose_data[1], fusepose_data[2], fusepose_data[3]);
            rpy = cv::Point3f(fusepose_data[4], fusepose_data[5], fusepose_data[6]);
        } 
        else if (line.find("ai_time:") != std::string::npos) {
            std::regex ai_time_regex(R"(\d+)");
            std::smatch ai_time_match;
            if (std::regex_search(line, ai_time_match, ai_time_regex)) {
                ai_time_stamp = std::stoll(ai_time_match.str());
            }
        } 
        else if (line.find("ai_obts:") != std::string::npos) {
            std::string ai_obts_data = line.substr(line.find(":") + 1);
            std::vector<cv::Point3f> xyz;
            std::regex ai_obts_regex(R"(\((.*?)\))");
            auto ai_obts_begin = std::sregex_iterator(ai_obts_data.begin(), ai_obts_data.end(), ai_obts_regex);
            auto ai_obts_end = std::sregex_iterator();

            for (std::sregex_iterator i = ai_obts_begin; i != ai_obts_end; ++i) {
                std::string point_str = (*i)[1].str();
                std::istringstream point_stream(point_str);
                std::string coord;
                std::vector<float> coords;

                while (std::getline(point_stream, coord, ',')) {
                    coords.push_back(std::stof(coord));
                }

                if (coords.size() == 3) {
                    if((coords[0] < 1.5f && coords[0] > 0.0f )&& (coords[1] < 1.5f && coords[1] > -1.5f) )
                        xyz.emplace_back(coords[0], coords[1], coords[2]);
                }
            }

            size_t pcl_no = xyz.size();
            long long diff = ai_time_stamp - lidar_time_stamp;

            // 將格式化數據添加到列表
            MatchedFrame frame = {
                lidar_time_stamp,
                ai_time_stamp,
                fusepose,
                rpy,
                xyz,
                diff,
                pcl_no
            };

            formatted_frames.push_back(frame);
        }
    }

    file.close();
    return formatted_frames;
}